#include "std_msgs/msg/u_int8.hpp"
#include "std_msgs/msg/u_int16_multi_array.hpp"
#include "std_msgs/msg/int16_multi_array.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "ubt_com/ubt_com_util.h"
#include "ubt_state/ubt_state_task_nav.h"
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/node.h"

namespace ubt_state
{
struct NavClsData
{
    float _obstacle_stop_dis;
    float _obstacle_slow_dis;
    float _obstacle_warn_dis;
    float _obstacle_side_dis;
    ObstacleParams obsParams;

    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr _pub_nav_speed_limit;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr _pub_real_route_info;
    rclcpp::Publisher<ubt_interface::msg::NavPathInfo>::SharedPtr _pub_nav_path_info;
    rclcpp::Publisher<ubt_interface::msg::NavTaskLimit>::SharedPtr _pub_nav_task_limit;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _sub_fixedpath_status;
#ifdef AVOID_CAR_SUPPORT
    rclcpp::Subscription<geometry_msgs::msg::Pose2D>::SharedPtr _sub_car_pose;
    float _car_check_dis;//检测距离
    int _car_check_appear_tm;//检测出现最小时长,毫秒
#endif

    CTaskNav *_pCurTask;
};
static NavClsData g_clsData;

static void _PubObstacleDis(float stopDis, float slowDis, float warnDis, float sideDis)
{
    ObstacleParams obsParams;
    obsParams.stopDis = stopDis;
    obsParams.slowDis = slowDis;
    obsParams.warnDis = warnDis;
    obsParams.sideDis = sideDis;
    UbtStateUtilSetObstacleParams(obsParams);
}

static void Fixedpath_Status_CallbackWrap(const std_msgs::msg::String::SharedPtr msg){
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Fixedpath_Status_Callback(msg);
    }
}
#ifdef AVOID_CAR_SUPPORT
/*
{
	"msgType": "",
	"timestamp": 12312412512,
	"data": {
		"agv_pose": {
			"x": 1.0,
			"y": 2.0,
			"t": 0.1
		},
		"obs_last_pose": {
			"x": 1.0,
			"y": 2.0,
			"t": 0.1
		},
		"train_arrive": {
			"obs_start_pose": {
				"x": 1.0,
				"y": 2.0,
				"t": 0.1
			},
			"obs_start_tm": 12312412512
		}
	}
}

msgType: 消息类型，train_arrive或train_leave;当为train_arrive时,data.train_arrive存在
agv_pose: AGV当前坐标
obs_last_pose: 小火车最近坐标
obs_start_pose: 小火车首次出现坐标
obs_start_tm: 首次出现时间
*/
char g_trainArriveFmt[] =
    "{\"msgType\":\"train_arrive\",\"timestamp\":%ld,\"data\":{\"agv_cur_pose\":{\"x\":%lf,\"y\":%lf,\"t\":%lf},\"obs_last_pose\":{\"x\":%lf,\"y\":%lf,\"t\":%lf},\"train_arrive\":{\"obs_start_pose\":{\"x\":%lf,\"y\":%lf,\"t\":%lf},\"obs_start_tm\":%ld}}}";
char g_trainLeaveFmt[] =
    "{\"msgType\":\"train_leave\",\"timestamp\":%ld,\"data\":{\"agv_cur_pose\":{\"x\":%lf,\"y\":%lf,\"t\":%lf},\"obs_last_pose\":{\"x\":%lf,\"y\":%lf,\"t\":%lf}}}";

void CarPose_CallbackWrap(const geometry_msgs::msg::Pose2D::SharedPtr msg){
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->CarPose_Callback(msg);
    }
}

void CTaskNav::SetCarParams(float checkDis, int waitTime)
{
    _checkDis2 = checkDis * checkDis;
    _waitTime = waitTime;
}

void CTaskNav::CarPose_Callback(const geometry_msgs::msg::Pose2D::SharedPtr msg)
{
    if (((_flag & 0x01) == 0) ||
            (_state >= ubt_task_state_stopping))
    {
        return;
    }
    int newCarState = 1;
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    if ((fabs(msg->theta) > 3.1416) ||	//非法值
            (msg->x != msg->x)) //nan
    {
        newCarState = 0;
    }
    else
    {
        if ((msg->x * msg->x + msg->y * msg->y) >= _checkDis2)
        {
            //检测距离之外
            newCarState = 0;
        }
        else
        {
            //转换坐标
            geometry_msgs::msg::Pose2D poseZero;
            poseZero.x = poseZero.y = poseZero.theta = 0.0;
            _carCurPose = ubt_com::getP2PoseInB(poseZero, pStatus->curPose, *msg);
        }
    }
    if (newCarState == _carState)
    {
        return;
    }
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::CarPose_Callback _carState=%d newCarState=%d msgPose=(%lf,%lf,%lf),_carCurPose=(%lf,%lf,%lf) curPose=(%lf,%lf,%lf)",
             _carState, newCarState,
             msg->x, msg->y, msg->theta,
             _carCurPose.x, _carCurPose.y, _carCurPose.theta,
             pStatus->curPose.x, pStatus->curPose.y, pStatus->curPose.theta);
    _carState = newCarState;
    _carStateTime = ubt_com::GetNowInMsecs();
    _CheckObstacle(3);
    memcpy(&_carStartPose, &_carCurPose, sizeof(geometry_msgs::msg::Pose2D));
    if (newCarState == 0)
    {
        _carCurPose.x = _carCurPose.y = _carCurPose.theta = 0.0;
        //有->无,立即上报
        if (_carLastReportTm != 0)
        {
            _NotifyCarStateChanged(_carStateTime);
        }
    }
    else
    {
        _carLastReportTm = 0;
    }
}
void CTaskNav::_NotifyCarStateChanged(long now)
{
    char buf[2048];
    int len;
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    len = sprintf(buf, "TRAIN_EVENT ");
    if (_carState == 1)
    {
        len += sprintf(buf + len, g_trainArriveFmt, now,
                       pStatus->curPose.x, pStatus->curPose.y, pStatus->curPose.theta,
                       _carCurPose.x, _carCurPose.y, _carCurPose.theta,
                       _carStartPose.x, _carStartPose.y, _carStartPose.theta, _carStateTime);
    }
    else
    {
        len += sprintf(buf + len, g_trainLeaveFmt, now,
                       pStatus->curPose.x, pStatus->curPose.y, pStatus->curPose.theta,
                       _carStartPose.x, _carStartPose.y, _carStartPose.theta);
    }
    _carLastReportTm = now;
    _pTaskCb->NotifyTaskEvent(std::string(buf, len));
}

#endif

int CTaskNav::ClassInit(ObstacleParams obsParams)
{
    NavClsData *pClsData = &g_clsData;
    pClsData->_obstacle_stop_dis = obsParams.stopDis;
    pClsData->_obstacle_slow_dis = obsParams.slowDis;
    pClsData->_obstacle_warn_dis = obsParams.warnDis;
    pClsData->_obstacle_side_dis = obsParams.sideDis;
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::ClassInit _stop_dis=%f _slow_dis=%f _warn_dis=%f",
             pClsData->_obstacle_stop_dis, pClsData->_obstacle_slow_dis, pClsData->_obstacle_warn_dis);

    pClsData->_pub_nav_speed_limit = node->create_publisher<std_msgs::msg::UInt8>("/nav_speed_limit", 1);
    pClsData->_pub_real_route_info = node->create_publisher<std_msgs::msg::Int32MultiArray>("/real_route_info", 1);
    pClsData->_pub_nav_path_info = node->create_publisher<ubt_interface::msg::NavPathInfo>("/nav_path_info", 1);
    pClsData->_pub_nav_task_limit = node->create_publisher<ubt_interface::msg::NavTaskLimit>("/nav_task_limit", 1);

    pClsData->_sub_fixedpath_status = node->create_subscription<std_msgs::msg::String>("/fixed_path_status", 10, std::bind(&Fixedpath_Status_CallbackWrap, std::placeholders::_1));
#ifdef AVOID_CAR_SUPPORT
    pClsData->_sub_car_pose = node->create_subscription<geometry_msgs::msg::Pose2D>("/car_pose", 1, std::bind(&CarPose_CallbackWrap, std::placeholders::_1));
    node->declare_parameter<float>("car_check_dis", 5.5);
    pClsData->_car_check_dis = node->get_parameter("car_check_dis").as_double();
    node->declare_parameter<int>("car_check_appear_tm", 3000);
    pClsData->_car_check_appear_tm = node->get_parameter("car_check_appear_tm").as_int();
#endif
    pClsData->_pCurTask = NULL;

    return 0;
}

void CTaskNav::ClassCleanup()
{
    NavClsData *pClsData = &g_clsData;
    // pClsData->_pub_nav_speed_limit->Shutdown();
    // pClsData->_pub_real_route_info->Shutdown();
    // pClsData->_pub_nav_path_info->Shutdown();
    // pClsData->_pub_nav_task_limit->Shutdown();
    // pClsData->_sub_fixedpath_status->Shutdown();
    pClsData->_pCurTask = NULL;
}

ITask *CTaskNav::CreateTask(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos, ubt_task_stage stage, int prePathAvoidHwMask,int index, int reportIndex)
{
    CTaskNav *pTask = new CTaskNav();
    if (!pTask->Init(pTaskCb, taskId, pLineInfos, stage, prePathAvoidHwMask,index,reportIndex))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskNav::~CTaskNav()
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::~CTaskNav _taskId=%s _state=%d", _taskId.c_str(), _state);
    if ((_state != ubt_task_state_idle) && (_state != ubt_task_state_closed))
    {
        _SetState(ubt_task_state_closed, -1, "destroy task wrong");
    }
}

bool CTaskNav::Init(ITaskCb *pTaskCb, std::string taskId, std::vector<Line> *pLineInfos, ubt_task_stage stage, int prePathAvoidHwMask,int index, int reportIndex)
{
    _pTaskCb = pTaskCb;
    _taskId = taskId;
	_index=index;
	_reportIndex=reportIndex;
    _pLineInfos = pLineInfos;
    _stage = stage;
    _prePathAvoidHwMask = prePathAvoidHwMask;
    _waitTime = 0;

    _state = ubt_task_state_idle;
    _stateTime = ubt_com::GetNowInMsecs();
	_flag = 0;
	
	_stopReason=0;
	_stopReasonDesc = "";
    
    _obstacleState = 0;
    _obstacleStateStime = 0;
    _avoidHwStatus = 0;
    _avoidDisable = 0;
    _avoidStage = 0;
    _pathIndex = -1;
    _closedReason = 0;
#ifdef AVOID_CAR_SUPPORT
    _checkDis2 = g_clsData._car_check_dis * g_clsData._car_check_dis;

    _carState = 0;
    _carStateTime = 0;
    _carStartPose.x = _carStartPose.y = _carStartPose.theta = 0.0;
    _carCurPose.x = _carCurPose.y = _carCurPose.theta = 0.0;
    _carLastReportTm = 0;
#endif
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Init  ok taskId=%s stage=%d prePathAvoidHwMask=%d", taskId.c_str(), stage, prePathAvoidHwMask);
    return true;
}

bool CTaskNav::Start()
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Start _taskId= %s _state=%d", _taskId.c_str(), _state);
    if (g_clsData._pCurTask != NULL)
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CTaskNav::Start fail curTask is not null, taskId=%s ", _taskId.c_str());
        return false;
    }
    if (_state != ubt_task_state_idle)
    {
        RCLCPP_WARN(node->get_logger(),"[ubt_state]CTaskNav::Start fail state=%d is not idle, taskId=%s", _state, _taskId.c_str());
        return false;
    }
    _SetState(ubt_task_state_running, 0, "recv cmd start");
    _OnArrive(-1);
    _ApplyLineProp(-1);
    return true;
}

void CTaskNav::Pause()
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Pause taskId= %s  _state=%d ", _taskId.c_str(), _state);
    if (_state == ubt_task_state_running)
    {
        _SetState(ubt_task_state_pause, 0, "recv cmd pause");
    }
}

void CTaskNav::Continue()
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Continue taskId= %s _state=%d  ", _taskId.c_str(), _state);
    if (_state == ubt_task_state_pause)
    {
#ifdef AVOID_CAR_SUPPORT
        _carState = 0; //重新开始检测小火车
        _carStateTime = ubt_com::GetNowInMsecs();
        _carLastReportTm = 0;
        _CheckObstacle(3);
#endif
        _SetState(ubt_task_state_running, 0, "recv cmd continue");
    }
}

void CTaskNav::Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Stop taskId= %s _state=%d reason=%d ", _taskId.c_str(), _state, reason);
    if (_state == ubt_task_state_closed)
    {
        return;
    }
    _SetState(ubt_task_state_closed, reason, "recv cmd stop");
}

void CTaskNav::_SetState(ubt_task_state newState, int reason, std::string reasonStr)
{
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_SetState newState=%d oldState=%d reason=%d reasonStr=%s",
             newState, _state, reason, reasonStr.c_str());
    if (_state == newState)
    {
        return;
    }
    int oldState = _state;
    _state = newState;
    _stateTime = ubt_com::GetNowInMsecs();
    if (_state == ubt_task_state_running)
    {
        if (oldState == ubt_task_state_idle)
        {
            g_clsData._pCurTask = this;
        }
    }
    if (_state == ubt_task_state_stopping)
    {
        _closedReason = reason;
        _closedReasonStr = reasonStr;
    }
    if (_state == ubt_task_state_closed)
    {
        g_clsData._pCurTask = NULL;
        _ApplyLineProp(-3);
    }
    _pTaskCb->NotifyTaskStateAndStage(GetState(), _stage, reason, reasonStr);
}

int CTaskNav::_SendNavInfo()
{
    int iRes = 0;
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    ubt_interface::msg::NavPathInfo pathMsg;
    pathMsg.task_id = _taskId;
    pathMsg.nav_type = "slam_nav";
    std::vector<Line>::iterator iterLine;
    for (iterLine = _pLineInfos->begin(); iterLine != _pLineInfos->end(); ++iterLine)
    {
        ubt_interface::msg::Line navLine;
        iRes = _Line2Nav(navLine, &*iterLine);
        if (iRes != 0)
        {
            break;
        }

        //如果是空载，发给导航空载最大限速
        if (pStatus->liftState == lift_state_down)
        {
            navLine.limit.x = iterLine->limit.x;
            navLine.limit.w = iterLine->limit.w;
        }
        //否则，发给导航负载最大限速
        else
        {
            navLine.limit.x = iterLine->limit.lx;
            navLine.limit.w = iterLine->limit.lw;
        }
        
        pathMsg.path.push_back(navLine);
    }
    if (iRes == 0)
    {
        g_clsData._pub_nav_path_info->publish(pathMsg);
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_SendNavInfo");
    }
    return iRes;
}

int CTaskNav::_Line2Nav(ubt_interface::msg::Line& navLine, Line *pLine)
{
    int iRes = 0;
    std::vector<Point>::iterator iterPt;
    ubt_interface::msg::Point navPoint;
    navLine.type = pLine->type;
    for (iterPt = pLine->points.begin(); iterPt != pLine->points.end(); ++iterPt)
    {
        navPoint.id = iterPt->id;
        navPoint.x = iterPt->x;
        navPoint.y = iterPt->y;
        navPoint.t = iterPt->t;
        navPoint.avoid_dis = iterPt->avoidDis;
        navLine.points.push_back(navPoint);
    }
    return iRes;
}

void CTaskNav::OnTimer()
{
    do
    {
        long now = ubt_com::GetNowInMsecs();
        if ((_state == ubt_task_state_idle) ||
                (_state == ubt_task_state_closed))
        {
            break;
        }

        if (_state == ubt_task_state_running)
        {
            if ((_flag & 0x01) == 0)
            {
                if ((now - _stateTime) < 500)
                {
                    break;
                }

                _OnAvoidStatusCb();
                _CheckObstacle(4);

                if (_obstacleState == 1)
                {
                    break;
                }

                // 发送路径给导航中间层节点
                // send path to navigation middle node
                _SendNavInfo();
                _flag |= 0x01;
            }
        }

#ifdef AVOID_CAR_SUPPORT
        if (_state < ubt_task_state_stopping)
        {
            if ((_carState == 1) && (_carLastReportTm == 0) && ((now - _carStateTime) >= g_clsData._car_check_appear_tm))
            {
                _NotifyCarStateChanged(now);
            }
            break;
        }
#endif
        if (_state == ubt_task_state_stopping)
        {
            if ((now - _stateTime) < _waitTime)
            {
                break;
            }
            _SetState(ubt_task_state_closed, _closedReason, _closedReasonStr);
        }
    }
    while (false);
}

void CTaskNav::OnMsg(int msgType, void *pvMsg)
{
    do
    {
        if ((_state == ubt_task_state_idle) ||
                (_state == ubt_task_state_closed) ||
                (_state == ubt_task_state_stopping))

        {
            break;
        }
        if (msgType == ubt_msg_type_current_pose)
        {
            _OnLocationCb();
            _CheckObstacle(2);
            break;
        }
        if (msgType == ubt_msg_type_avoid_status)
        {
            _OnAvoidStatusCb();
            _CheckObstacle(1);
            break;
        }
        if (msgType == ubt_msg_type_map_confidence)
        {
            float poseCfd = *(float *)pvMsg;
            if (poseCfd < 0.0001)
            {
                _SetState(ubt_task_state_stopping, ubt_task_result_sys_posefail, UBT_TASK_EVENT_SYS_POSEFAIL_STR);
            }
        }
        if (msgType == ubt_msg_type_nav_limit)
        {
            NavTaskLimitIndex *taskLimit = (NavTaskLimitIndex *)pvMsg;
            _PubNavLimit(taskLimit);
            break;
        }
    }
    while (false);
}

ubt_task_type CTaskNav::GetType()
{
    return ubt_task_type_nav;
}

ubt_task_state CTaskNav::GetState()
{
    ubt_task_state state = _state;
    if (state == ubt_task_state_running)
    {
        if (_obstacleState == 1)
        {
            state = ubt_task_state_obstacle;
        }
    }
    return state;
}


ubt_task_stage CTaskNav::GetStage()
{
    return _stage;
}


std::string CTaskNav::GetActionType(){
	return "nav";
}

void CTaskNav::GetIndex(int& index, int& reportIndex){
    index = _index;
    reportIndex = _reportIndex;
}

void CTaskNav::GetStopInfo(int& stopReason, std::string& stopReasonDesc){
    stopReason = _stopReason;
    stopReasonDesc = _stopReasonDesc;
}

void CTaskNav::GetLogs(std::vector<TaskLogItem>& logs){
}

//处理路径的返回状态
void CTaskNav::Fixedpath_Status_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    std::string msgData = msg->data;
    int closeReason;
    if (_oldMsgData != msgData)
    {
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::Fixedpath_Status_Callback  taskId= %s msg= %s",
                 _taskId.c_str(), msgData.c_str());
        _oldMsgData = msgData;
    }
    do
    {
        if ((_state == ubt_task_state_idle) ||
                (_state == ubt_task_state_closed) ||
                (_state == ubt_task_state_stopping))
        {
            break;
        }
        if (!memcmp(msgData.c_str(), (char *)"TASK_PROGRESS", 13))
        {
            std::vector<std::string> vals = ubt_com::SpitStr(msgData, ' ');
            if (vals.size() < 2)
            {
                break;
            }
            int pathIndex = atoi(vals[1].c_str());
            if (pathIndex < 0)
            {
                break;
            }

            _OnArrive(pathIndex);
            break;
        }
        if (!memcmp(msgData.c_str(), (char *)"TASK_ROTATE", 11))
        {
            std::vector<std::string> vals = ubt_com::SpitStr(msgData, ' ');
            if (vals.size() < 2)
            {
                break;
            }
            int pathIndex = atoi(vals[1].c_str());
            if ((pathIndex < 0) || (_pathIndex != pathIndex))
            {
                break;
            }
            _ApplyLineProp(pathIndex);
            break;
        }
        std::string reasonStr;
        if (msgData == "TASK_FINISH")
        {
            _OnArrive(-2);
            closeReason = ubt_task_result_ok;
            reasonStr = UBT_TASK_EVENT_ARRIVED_STR;
            _pTaskCb->NotifyTaskEvent(UBT_TASK_EVENT_ARRIVED_STR);
        }
        else if (msgData == "TASK_ABORT")
        {
            closeReason = ubt_task_result_nav_abort;
            reasonStr = UBT_TASK_EVENT_NAV_ABORT_STR;
        }
        else if (msgData == "TASK_ABORT_EXCEED_3M")
        {
            closeReason = ubt_task_result_nav_aborte3m;
            reasonStr = UBT_TASK_EVENT_NAV_ABORTE3M_STR;
        }
        else if (msgData == "TASK_ABORT_POSE_JUMP")
        {
            closeReason = ubt_task_result_nav_posejump;
            reasonStr = UBT_TASK_EVENT_NAV_POSEJUMP_STR;
        }
        else if (msgData == "TASK_ABORT_TRACK_FAILED")
        {
            closeReason = ubt_task_result_nav_trackfail;
            reasonStr = UBT_TASK_EVENT_NAV_TRACKFAIL_STR;
        }
        else
        {
            break;
        }
        _SetState(ubt_task_state_stopping, closeReason, reasonStr);
    }
    while (false);
}

void CTaskNav::_OnLocationCb()
{
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    geometry_msgs::msg::Pose2D *pCurPos = &pStatus->curPose;
    Point *pPtNext = NULL;
    do
    {
        if ((_pathIndex < -1) ||
                (_pathIndex >= (int)_pLineInfos->size()))
        {
            break;
        }
        float stopDis, slowDis, warnDis, sideDis;
        if (_pathIndex == -1)
        {
            pPtNext = &(*_pLineInfos)[0].points[0];
            stopDis = g_clsData._obstacle_stop_dis;
            slowDis = g_clsData._obstacle_slow_dis;
            warnDis = g_clsData._obstacle_warn_dis;
            sideDis = g_clsData._obstacle_side_dis;
        }
        else
        {
            pPtNext = &(*_pLineInfos)[_pathIndex].points[1];
            stopDis = (*_pLineInfos)[_pathIndex].stop;
            slowDis = (*_pLineInfos)[_pathIndex].slow;
            warnDis = (*_pLineInfos)[_pathIndex].warning;
            sideDis = (*_pLineInfos)[_pathIndex].side;
        }
        if ((pPtNext->avoidDis < 0) ||
                ((pPtNext->avoidDis >= stopDis) && (pPtNext->avoidDis >= sideDis)))
        {
            break;
        }
        if (ubt_com::isZero(pPtNext->avoidDis))
        {
            if (_avoidStage == 2)
            {
                break;
            }
        }
        else
        {
            if (_avoidStage != 0)
            {
                break;
            }
        }
        stopDis += 0.1 + pStatus->expLength / 2;
        double	xOff = fabs(pCurPos->x - pPtNext->x), yOff = fabs(pCurPos->y - pPtNext->y), dis2;
        if ((xOff > stopDis) ||
                (yOff > stopDis))
        {
            break;
        }
        dis2 = xOff * xOff + yOff * yOff;
        if (dis2 > (stopDis * stopDis))
        {
            break;
        }
        if (ubt_com::isZero(pPtNext->avoidDis))
        {
            if (dis2 < 0.02) //0.14米处，关闭避障
            {
                _avoidDisable = 1;
                break;
            }
            if (_avoidStage == 1) //已经设置过避障参数
            {
                break;
            }
            stopDis = 0.1;
            sideDis = 0.1;
        }
        else
        {
            if (stopDis > pPtNext->avoidDis)
            {
                stopDis = pPtNext->avoidDis;
            }
            if (sideDis > pPtNext->avoidDis)
            {
                //站点避障距离需影响侧面
                sideDis = pPtNext->avoidDis;
            }
        }
        //设置避障参数
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_OnLocationCb  send obstacle_dis _taskId= %s pathIndex=%d dis=(%f,%f,%f,%f) curPose=(%lf,%lf)",
                 _taskId.c_str(), _pathIndex, stopDis, slowDis, warnDis, sideDis, pCurPos->x, pCurPos->y);
        _PubObstacleDis(stopDis, slowDis, warnDis, sideDis);
        _avoidStage = 1;
    }
    while (false);
}

void  CTaskNav::_OnAvoidStatusCb()
{
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    _avoidHwStatus = pStatus->avoidHwStatus;
    if (_pathIndex == -1)
    {
        if (_prePathAvoidHwMask & avoid_hw_mask_rgbd)
        {
            _avoidHwStatus = pStatus->laserAvoidStatus;
        }
    }
}
//到达上报点位
void CTaskNav::_OnArrive(int pathIndex)
{
    //-2 到达，-1 出发，非负数-路网上第几段(0起)
    if ((pathIndex >= (int)_pLineInfos->size()) ||
            (pathIndex < -2) ||
            (pathIndex == _pathIndex))
    {
        return;
    }
    _pathIndex = pathIndex;
    _avoidStage = 2;
    // std_msgs::msg::Int16MultiArray pathMsg;
    std_msgs::msg::Int32MultiArray pathMsg;
    Line *pLine;
    Point *pPt1, *pPt2;
    unsigned short id1, id2;
    if (_pathIndex >= 0)
    {
        pLine = &(*_pLineInfos)[_pathIndex];
        pPt1 = &pLine->points[0];//0 is first point
        pPt2 = &pLine->points[1];//1 is last point
        id1 = pPt1->id;
        id2 = pPt2->id;
    }
    else
    {
        if (-1 == _pathIndex)
        {
            pLine = &(*_pLineInfos)[0];
            pPt1 = &pLine->points[0];
            id1 = 0;
            id2 = pPt1->id;
        }
        else//=-2
        {
            pLine = &(*_pLineInfos)[_pLineInfos->size() - 1];
            pPt2 = &pLine->points[1];
            id1 = pPt2->id;
            id2 = 0;
        }
    }
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_OnArrive _taskId= %s _pathIndex=%d id1=%d -> id2=%d",
             _taskId.c_str(), _pathIndex, id1, id2);
    pathMsg.data.push_back(id1);
    pathMsg.data.push_back(id2);
    pathMsg.data.push_back(_pathIndex);
    g_clsData._pub_real_route_info->publish(pathMsg);
}

void CTaskNav::_ApplyLineProp(int pathIndex)
{
    //-3 结束,-1 出发，非负数-路网上第几段
    if ((pathIndex >= (int)_pLineInfos->size()) ||
            (pathIndex < -3))
    {
        return;
    }
    // std_msgs::msg::UInt16MultiArray pathMsg;
    std_msgs::msg::Int32MultiArray pathMsg;
    Line *pLine;
    Point *pPt1, *pPt2;
    float stopDis, slowDis, warnDis, sideDis;
    if (pathIndex >= 0)
    {
        pLine = &(*_pLineInfos)[pathIndex];
        stopDis = pLine->stop;
        slowDis = pLine->slow;
        warnDis = pLine->warning;
        sideDis = pLine->side;
    }
    else
    {
        stopDis = g_clsData._obstacle_stop_dis;
        slowDis = g_clsData._obstacle_slow_dis;
        warnDis = g_clsData._obstacle_warn_dis;
        sideDis = g_clsData._obstacle_side_dis;
    }
    _avoidDisable = 0;
    _avoidStage = 0;
    RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_ApplyLineProp  send obstacle_dis _taskId= %s pathIndex=%d dis=(%f,%f,%f,%f)",
             _taskId.c_str(), pathIndex, stopDis, slowDis, warnDis, sideDis);
    _PubObstacleDis(stopDis, slowDis, warnDis, sideDis);
}

void CTaskNav::_CheckObstacle(int event)
{
    int oldObstcalState = _obstacleState;
    if (_avoidDisable != 0)
    {
        _obstacleState = 0;
    }
    else
    {
        _obstacleState = _avoidHwStatus;
#ifdef AVOID_CAR_SUPPORT
        if ((_obstacleState == 0) || (_obstacleState == 3))
        {
            if (_carState == 1)
            {
                _obstacleState = 2;
            }
        }
#endif
    }
    if (_obstacleState != oldObstcalState)
    {
        _obstacleStateStime = ubt_com::GetNowInMsecs();
        RCLCPP_INFO(node->get_logger(),"[ubt_state]CTaskNav::_CheckObstacle _taskId= %s _obstacleState=%d event=%d oldObstcalState=%d",
                 _taskId.c_str(), _obstacleState, event, oldObstcalState);
        std_msgs::msg::UInt8 msg;
        msg.data = _obstacleState;
        RCLCPP_INFO_STREAM(node->get_logger(),"[ubt_state]CTaskNav::_CheckObstacle send nav_speed_limit _taskId= " << _taskId.c_str() << "  msg: " << msg.data);
        g_clsData._pub_nav_speed_limit->publish(msg);
        _pTaskCb->NotifyTaskStateAndStage(GetState(), _stage, 0, "");
    }
}

void CTaskNav::_PubNavLimit(NavTaskLimitIndex *taskLimit)
{
    ubt_interface::msg::NavTaskLimit taskLimitMsg;
    taskLimitMsg.task_id = taskLimit->taskId;
    taskLimitMsg.nav_type = "slam_nav";
    taskLimitMsg.wait_line_index = taskLimit->waitLineIndex;
    taskLimitMsg.wait_point_id = taskLimit->waitPointId;
    taskLimitMsg.cmd = taskLimit->cmd;
    g_clsData._pub_nav_task_limit->publish(taskLimitMsg);
    
    RCLCPP_INFO_STREAM(node->get_logger(),"[ubt_state]CTaskNav::_PubNavLimit taskId= " << taskLimit->taskId << " waitLineIndex: " << taskLimit->waitLineIndex << " waitPointId: " << taskLimit->waitPointId << " cmd: " << taskLimit->cmd);
}

}



